#include "app.h"

// #######################################################################################################
// KEYBOARD
// #######################################################################################################
void onKeyDown(char key) {
	lprintf("onKeyDown key [0x%x]"CMD_TERMINATOR, key);
	if (key == SW0) beep_Play(4);
	else if (key == SW1) beep_Play(5);
	else if (key == SW2) beep_Play(6);
}

void onKeyPress(char key __attribute__ ((unused)) ) {
	lprintf("onKeyPress key [0x%x] fastInc[%d] "CMD_TERMINATOR, key, gkey_FastInc);
}

void onKeyUp(char key __attribute__ ((unused)) ) {
	lprintf("onKeyUp key [0x%x] fastInc[%d] "CMD_TERMINATOR, key, gkey_FastInc);
}

// #######################################################################################################
// RELAYS
// #######################################################################################################
void relay_Init(void) {
	relay1_Init();
	relay2_Init();

	control.relay1.st = 0;
	control.relay1.timeout = 0;

	control.relay2.st = 0;
	control.relay2.timeout = 0;
}

// Liga ou desliga o rele
//	parametro rly: sinaliza qual o rele vamos acessar. valor 0 ou 1
//	parametro st: pdON liga o rele e pdOFF desliga o rele;
//	parametro timeout: O tempo em segundos que o rele vai ficar no estado atual e voltar para o estado inverso
//		Se colocar valor 0, permanece no estado atual até o próximo comando
int relay_Set(int rly, int st, uint timeout) {
	pRelay rl;

	if ( (rly < 0) || (rly > 1) ) return errDEVICE_NOT_FOUND;
	if (rly == 0) rl = &control.relay1; else rl = &control.relay2;

	rl->timeout = timeout*1000;
	rl->st = st;

	return pdPASS;
}

void relay_Tick(void) {
	if (control.relay1.st) relay1_On(); else relay1_Off();
	if (control.relay2.st) relay2_On(); else relay2_Off();
	if (control.relay1.timeout > 0) control.relay1.timeout--;
	if (control.relay2.timeout > 0) control.relay2.timeout--;
	//colocar depois do acionamento para que seja certinho a contagem dos tempos
	if (control.relay1.timeout == 1) control.relay1.st ^= 1;
	if (control.relay2.timeout == 1) control.relay2.st ^= 1;
}

// #######################################################################################################
// SERVOS
// #######################################################################################################
volatile uint servosTick = 0;
void servo_Init(void) {
	uint x;	for (x=0;x<SERVO_N_MODULE;x++) {
		control.servo[x].countProgress = 0;
		control.servo[x].timeActiveCount = 0;
		//control.servo[x].pos = parServo->init;
		//control.servo[x].posNew = control.servo[x].pos;
		control.servo[x].time = 0;
		//control.servo[x].pos = 0;
	}

	servo_Parking();
}

// podemos acionar o servo de modo que a posição ou movimento se faz de modo direto ou progressivo em ms
//	servo: o servo alvo para posicionamento, valor de 1 a 7
// offset: deve ser multiplo de 10
// tp (ms). Valor 0: não há modo progressivo para o servo. Valor > 0 faz modo progressivo
// toff: tempo em segundos que o servo vai ficar ativo. Valor 0 sinaliza ficar ligado direto
int servo_Config(uint servo, uint init, uint min, uint max, int offset, int tp, int toff) {
	if ( (servo == 0) || (servo > SERVO_N_MODULE) ) return errDEVICE_NOT_FOUND;
	if ( (offset<minOFFSET_US) || (offset>maxOFFSET_US) ) return errDEVICE_VALUE_INVALID;
	if ( (init<minSERVO_US) || (init>maxSERVO_US) ) return errDEVICE_VALUE_INVALID;
	if ( (min<minSERVO_US) || (min>maxSERVO_US) ) return errDEVICE_VALUE_INVALID;
	if ( (max<=min) || (max>maxSERVO_US) ) return errDEVICE_VALUE_INVALID;

	if(tp<0) tp=0;
	if(toff<0) toff=0;

	tParServo* parServo = (tParServo*)control.servo[servo-1].par;
	parServo->offset = (offset/10)*10;// deve ser multiplo de 10 por causa do time progresse em 10 em 10
	parServo->min = min;
	parServo->max = max;
	parServo->timeProgress = tp;
	parServo->timeActiveOff = toff;

	// o servo 3 é o espelho do servo 2
	if (servo == 2)	{
		parServo->init = init;
		parServo = (tParServo*)control.servo[servo].par;
		parServo->init = (1500-(init-1500));
	} else if (servo != 3)
		parServo->init = init;

	flash_Save();

	return pdPASS;
}

// podemos acionar o servo de modo que a posição ou movimento se faz de modo direto ou progressivo em ms
//	servo: o servo alvo para posicionamento, valor de 1 a 7
// offset: deve ser multiplo de 10
int servo_Offset(uint servo, int offset) {
	if ( (servo == 0) || (servo > SERVO_N_MODULE) ) return errDEVICE_NOT_FOUND;
	if ( (offset<minOFFSET_US) || (offset>maxOFFSET_US) ) return errDEVICE_VALUE_INVALID;

	tParServo* parServo = (tParServo*)control.servo[servo-1].par;
	parServo->offset = (offset/10)*10;// deve ser multiplo de 10 por causa do time progresse em 10 em 10
	flash_Save();

	return pdPASS;
}

// Ajusta o posicionamento do servo desejado
//	servo: o servo alvo para posicionamento, valor de 1 a 7
//  us: largura do pulso em microsegundos de controle de posição do servos. Valor entre 100 a 3000uS
int servo_SetPos(uint servo, uint us) {
	if ( (servo==0) || (servo>SERVO_N_MODULE) ) return errDEVICE_NOT_FOUND;
	tParServo* parServo = (tParServo*)control.servo[servo-1].par;
	if ( (us<(uint)parServo->min) || (us > (uint)parServo->max) ) return errDEVICE_VALUE_OUT_OF_RANGE;
	if ( (us<minSERVO_US) || (us>maxSERVO_US)) return errDEVICE_VALUE_INVALID;

	// o servo 3 será o espelho do servo 2
	if (servo == 3) return pdPASS;

	control.servo[servo-1].posNew = us + parServo->offset;
	control.servo[servo-1].countProgress = parServo->timeProgress;
	control.servo[servo-1].time = servosTick;
	control.servo[servo-1].timeActiveCount = 0;
	control.servo[servo-1].timeActiveOff = parServo->timeActiveOff*1000; // Multiplicar por 1000 porque o tempo de controle é em 1ms
	servus_Enable(servo, 1); // Liga o servo

	// o servo 3 será o espelho do servo 2
	if (servo == 2) {
		us = control.servo[servo-1].posNew;
		control.servo[servo].posNew = (1500-(us-1500));
		parServo = (tParServo*)control.servo[servo].par;
		control.servo[servo].posNew+=parServo->offset;
		control.servo[servo].countProgress = control.servo[servo-1].countProgress;
		control.servo[servo].time = control.servo[servo-1].time;
		control.servo[servo].timeActiveCount = 0;
		control.servo[servo].timeActiveOff = control.servo[servo-1].timeActiveOff;
		servus_Enable(servo+1, 1); // Liga o servo
	};

	return pdPASS;
}

void servo_Parking(void) {
	tParServo* parServo;
	uint x;	for (x=0;x<SERVO_N_MODULE;x++) {
		parServo = (tParServo*)control.servo[x].par;
		control.servo[x].posNew = parServo->init+parServo->offset;
		control.servo[x].pos = control.servo[x].posNew;
		servus_SetPos(x+1, control.servo[x].posNew);
		servus_Enable(x+1, 1); // Liga o servo
	}

	servo_Hand(handOPEN);
}

void servo_Hand(int act) {
	tParServo* parServo = (tParServo*)control.servo[6].par;
	if (act == handCLOSE) servo_SetPos(7, parServo->max);
	else servo_SetPos(7, parServo->min);
}

#if USE_TIME_PROGRESS	pdOFF
void servo_Tick(void) {
	servosTick++;

	// fazer para todos os servos
	uint x;	for (x=0;x<SERVO_N_MODULE;x++) {
		// se o servo está desligado vamos ignorar
		if (servus_Enabled(x+1)==pdOFF) continue;

		// se a posição for diferente vamos atualizar o servo
		if (control.servo[x].pos != control.servo[x].posNew) {
			if (control.servo[x].countProgress) { // se for de modo progressivo vamos mudar em 10 us a posisção a cada countProgress
				// se não estourou o tempo de progressão do servo, não faz nada
				if (control.servo[x].time + control.servo[x].countProgress > servosTick) continue;
				control.servo[x].time = servosTick; // atauliza o tempo
				// nova posição do servo de acordo com a posição atual
				if (control.servo[x].pos < control.servo[x].posNew) control.servo[x].pos += 10; else control.servo[x].pos -= 10;
			} else control.servo[x].pos = control.servo[x].posNew; // se não for modo progressivo, troca imediatamente

			servus_SetPos(x+1, control.servo[x].pos); 	// Atualizar a posição do servo
			control.servo[x].timeActiveCount = 0; 		// Sinaliza que o servo está ainde se posicionando, então não vamos contar o tempo para desligá-lo

		// se está na posição correta, vamos determinar o tempo de acionamento do mesmo caso foi especificado
		} else {
			control.servo[x].countProgress = 0;
			control.servo[x].timeActiveCount++;

			// Desliga o servo caso foi especificado o tempo para manter ligado
			if ( (control.servo[x].timeActiveOff > 0) && (control.servo[x].timeActiveCount > control.servo[x].timeActiveOff) )
	 			servus_Enable(x+1, 0); // desliga o servo
		}
	}
}
